
/*
 * =====================================================================================
 *
 *       Filename:  sph_lib.c
 *
 *    Description:  
 *
 *        Created:  04/28/2009 04:51:24 PM EDT
 *         Author:  Dinesh Kumar (dkumar), dkumar@buffalo.edu
 *        License:  GNU General Public License
 *
 * This software can be redistributed free of charge.  See COPYING
 * file in the top distribution directory for more details.
 *
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 * =====================================================================================
 * $Id:$
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#ifdef HAVE_MPI_H
#  include <mpi.h>
#endif

#include <cmath>
using namespace std;

#include "particle.h"
#include "sph_header.h"
#include "constants.h"

//! calculate dot-product
double
dot(double vec1[], double vec2[])
{
  double sum = 0.;
  for (int i = 0; i < DIMENSION; i++)
    sum += vec1[i] * vec2[i];
  return sum;
}


/*! Rotate vector of state vars {\it i.e.}
 * $\mathbf{u} = (\mathbf{u} \cdot \hat{n}) \hat{n}$
 */
void
rotate (double * u, double * cos)
{
  // u_r = dot (u, n_r) * n_r
  int i;
  double temp = dot (u, cos);

  for (i = 0; i < DIMENSION; i++)
    u[i] = temp * cos[i];
  return;
}

/*! reflect vector $\mathbf{u}_i$ in plane $\hat{n}$
 * $\mathbf{u}_r = \mathbf{u}^i - 2 (\mathbf{u}^i \cdot \hat{n})\hat{n}$
 */
void
reflect (double * ui, double * ur, double * n)
{
  int i;
  double vdotn = dot (ui, n);
  for (i = 0; i < DIMENSION; i++)
    ur[i] = ui[i] - 2 * vdotn * n[i];
  return;
}

/****************************************************
 * h is the smoothing length
 * the weight extends up to 3*h in all directions
 ***************************************************/
const double cutoff = 3.0;
const double t1 = 0.564189583547756;    // 1/sqrt(pi)

// weight function
double
weight(double *s, double h)
{
  int i;
  double norm, expt, w;

  for (i = 0; i < DIMENSION; i++)
    if (abs(s[i]) > cutoff)
      return 0;

  norm = 1;
  expt = 0;
  for (i = 0; i < DIMENSION; i++)
  {
    norm *= t1 / h;
    expt += s[i]*s[i];
  }
  w = norm * exp(-expt);
  return w;
}

/****************************************************
 * d_weight is derivative of weight function in 
 * any given direction
 * x-dir = 0
 * y-dir = 1
 * z-dir = 2
 ****************************************************/
// d(weight function)
double
d_weight (double *s, double h, int dir)
{
  int i;
  double dw, tmp;

  for (i = 0; i < DIMENSION; i++)
    if (abs (s[i]) > cutoff)
      return 0;

  tmp = -2 * s[dir] / h;
  dw = tmp * weight (s, h);
  return dw;
}

void
matrix_vec_mult(double *A, int N, double *b, double *c)
{
  register int i, j;
  for (i = 0; i < N; i++)
    c[i] = 0;

  for (i = 0; i < N; i++)
    for (j = 0; j < N; j++)
      c[i] += A[i * N + j] * b[j];
  return;
}

void
matrix_matrix_mult (double * A,   //! input A : N x P matrix
                    int N,        //! no. of rows of LHS
                    int P,        //! no. of columns of LHS
                    double * B,   //! input B : P x M matrix
                    int M,        //! no. of columns of RHS matrix
                    double * C    //! output C: N x M matrix
                   )
{
  register int i, j, k;

  for (i = 0; i < N; i++)
    for (j = 0; j < M; j++)
      C[i * M + j] = 0;

  for (i = 0; i < N; i++)
    for (j = 0; j < M; j++)
      for (k = 0; k < P; k++)
        C[i * M + j] += A[i * P + k] * B[k * M + j];

  return;
}

// code generated by Maple ( assummed correct ) 
void
inv3 (double * A, //! input: 3 x 3 Matrix
      double * Ap //! output: 3 x 3 Matrix inverse
     )
{
  double t4 = A[2] * A[3];
  double t6 = A[2] * A[6];
  double t8 = A[1] * A[3];
  double t10 = A[1] * A[6];
  double t12 = A[0] * A[4];
  double t14 = A[0] * A[7];
  double t17 =
    1.0 / (t4 * A[7] - t6 * A[4] - t8 * A[8] + t10 * A[5] + t12 * A[8] -
           t14 * A[5]);
  Ap[0] = (A[4] * A[8] - A[7] * A[5]) * t17;
  Ap[1] = -(A[3] * A[8] - A[6] * A[5]) * t17;
  Ap[2] = (A[3] * A[7] - A[6] * A[4]) * t17;
  Ap[3] = -(-A[2] * A[7] + A[1] * A[8]) * t17;
  Ap[4] = (-t6 + A[0] * A[8]) * t17;
  Ap[5] = -(-t10 + t14) * t17;
  Ap[6] = (-A[2] * A[4] + A[1] * A[5]) * t17;
  Ap[7] = -(-t4 + A[0] * A[5]) * t17;
  Ap[8] = (-t8 + t12) * t17;

}

// solve linear equations for LHS: 3 x 3 and RHS: 3 x N
void 
linsolve (double * A,  //! input: LHS 3 x 3 Matrix
          double * d,  //! input: RHS 3 x N Matix
          int N,       //! number of columns in RHS
          double * x   //! output  3 x N Matrix
         )
{
  double Ainv[9];
  inv3 (A, Ainv);
  matrix_matrix_mult (Ainv, 3, 3, d, N, x);
  return;
}


// linear solver for 3 x 3 matrix and 3 x 1 vector
void
linsolve31 (double * A, double * d, double * sol )
{
  double t1 = A[4] * A[8] - A[5] * A[7];
  double t2 = A[1] * A[5] - A[2] * A[4];
  double t3 = -A[1] * A[8] + A[2] * A[7];
  double t4 = t3 * A[3] + t2 * A[6] + A[0] * t1;
  t4 = 1. / t4;
  sol[0] = (t1 * d[0] + t3 * d[1] + t2 * d[2]) * t4;
  sol[1] = (-(A[3] * A[8] - A[5] * A[6]) * d[0] + 
           (A[0] * A[8] - A[6] * A[2]) * d[1] - 
           (A[0] * A[5] - A[2] * A[3]) * d[2]) * t4;
  sol[2] = ((A[3] * A[7] - A[4] * A[6]) * d[0] - 
            (A[0] * A[7] - A[6] * A[1]) * d[1] + 
            (A[0] * A[4] - A[3] * A[1]) * d[2]) * t4;
  return;
}


/* 
 * z = p1 x + p2 y + p3 x y + p4 
 */
void poly_surf (double * x, double * y, double * z, double * poly)
{
   
  // pt0 (x=0, y=0)
  poly[3] = z[0];

  // pt1 (x = dx, y = 0)
  poly[0] = (z[1] - poly[3]) / x[1];

  // pt3 (x = 0, y = dy)
  poly[1] = (z[3] - poly[3]) / y[1];

  // pt2 (x = dx, y = dx)
  poly[2] = (z[2] - (poly[0] * x[1] + poly[1] * y[1] + poly[3])) / 
            (x[1] * y[1]);
  return;
}


/* 
 * fit least square surface with 3 constants
 * z = p1 x + p2 y + p3 
 */
void
lsq_surf3 (double * x, double * y, double * z, double * poly)
{
  double t1 = y[0] * y[0];
  double t2 = 2.;
  double t3 = y[1] * y[1];
  double t4 = x[1] * x[1];
  double t5 = x[2] * x[2];
  double t6 = x[0] * x[0];
  double t7 = y[2] * y[2];
  double t8 = x[0] * x[2];
  double t9 = x[1] * x[2];
  double t10 = x[0] * x[1];
  double t11 = (x[0] + x[1] - x[2]) * x[2];
  double t12 = (t3 + t1) * t5;
  double t13 = (t1 + t7) * t4 + t12 + t6 * (t7 + t3) + 
               (-t9 * t1 + (y[0] * (-t10 + t11) - t8 * y[1]) * y[1] + 
               ((-t8 + (x[2] + x[0] - x[1]) * x[1]) * y[0] + 
                (-t9 + (x[2] + x[1] - x[0]) * x[0]) * y[1] - 
                t10 * y[2]) * y[2]) * t2;
  double t14 = x[0] * z[0] + x[1] * z[1] + x[2] * z[2];
  double t15 = t2 * y[1];
  double t16 = (t15 - y[0] - y[2]) * x[1] + 
               (t2 * y[2] - y[0] - y[1]) * x[2] + 
               (t2 * y[0] - y[1] - y[2]) * x[0];
  double t17 = y[0] * z[0] + y[1] * z[1] + y[2] * z[2];
  double t18 = y[0] * y[1];
  double t19 = y[1] * y[2];
  double t20 = y[0] * y[2];
  t7 = (t18 + t19 - t1 - t7) * x[1] + 
       (t20 + t19 - t1 - t3) * x[2] + 
       (t18 + t20 - t3 - t7) * x[0];
  t18 = z[0] + z[1] + z[2];
  t13 = 1. / t13;
  t5 = (t4 + t5 - t10 - t8) * y[0] + 
       (t6 + t4 - t8 - t9) * y[2] + 
       (t6 + t5 - t10 - t9) * y[1];

  // solution
  poly[0] = (t2 * (t1 + (-y[0] + y[1]) * y[1] + 
            (-y[0] - y[1] + y[2]) * y[2]) * t14 - t16 * t17 + t7 * t18) * t13;
  poly[1] = (-t16 * t14 + t2 * (t6 + (-x[0] + x[1]) * x[1] - t11) * 
             t17 - t5 * t18) * t13;
  poly[2] = (t7 * t14 - t5 * t17 + 
             (t4 * t1 - t15 * t10 * y[0] + t12 + 
             t6 * t3 + (-t2 * x[2] * (x[0] * y[0] + x[1] * y[1]) 
                        + (t6 + t4) * y[2]) * y[2]) * t18) * t13;
  return;
}
